/**
 * @file	Test_Mouse_Radio.c
 * @author	Mitch, David
 * @brief
 */

#include "../header/common.h"
#include "../roomba/roomba.h"
#include "../roomba/roomba_sci.h"
#include "../uart/uart.h"
#include "../radio/radio.h"
#include <util/delay.h>

#ifdef TEST_RADIO_MOUSE_BUMPER

#define     clock8MHz()    CLKPR = _BV(CLKPCE); CLKPR = 0x00;

typedef enum _ir_dir
{
	IR_NORTH,
	IR_EAST,
	IR_SOUTH,
	IR_WEST,
	IR_NONE,
} IR_Direction_t;

IR_Direction_t direction = IR_NONE;

/** Get the North sensor reading.
 * @return 1 if sensor detects an IR signal, 0 if it does not.
 */
uint8_t get_IR_North()
{
	// if the N pin is low, then sensor detects an IR signal
	return (PINA & _BV(PORTA7)) == 0;
}

/** Get the East sensor reading.
 * @return 1 if sensor detects an IR signal, 0 if it does not.
 */
uint8_t get_IR_East()
{
	// if the E pin is low, then sensor detects an IR signal
	return (PINA & _BV(PORTA5)) == 0;
}

/** Get the South sensor reading.
 * @return 1 if sensor detects an IR signal, 0 if it does not.
 */
uint8_t get_IR_South()
{
	// if the S pin is low, then sensor detects an IR signal
	return (PINA & _BV(PORTA3)) == 0;
}

/** Get the South sensor reading.
 * @return 1 if sensor detects an IR signal, 0 if it does not.
 */
uint8_t get_IR_West()
{
	// if the W pin is low, then sensor detects an IR signal
	return (PINA & _BV(PORTA1)) == 0;
}

IR_Direction_t get_current_direction()
{
	// according to the Pololu documentation, only one line should
	// be high at a time.
	uint8_t input_reg = PINA;
	if ((input_reg & _BV(PORTA1)) == 0) return IR_WEST;
	if ((input_reg & _BV(PORTA3)) == 0) return IR_SOUTH;
	if ((input_reg & _BV(PORTA5)) == 0) return IR_EAST;
	if ((input_reg & _BV(PORTA7)) == 0) return IR_NORTH;

	return IR_NONE;
}

const unsigned char PPP[] = {};
const unsigned int PT = sizeof(PPP)/2;
void roomba_Drive( int16_t velocity, int16_t radius )
{
	uart_putchar(DRIVE);
	uart_putchar(HIGH_BYTE(velocity));
	uart_putchar(LOW_BYTE(velocity));
	uart_putchar(HIGH_BYTE(radius));
	uart_putchar(LOW_BYTE(radius));
}

uint8_t station1_addr[5] = { 0xED, 0xB7, 0xED, 0xB7, 0xED };
volatile uint8_t rxflag = 0;
radiopacket_t packet;

int get_bumper_status() {
	uart_putchar(SENSORS);
	uart_putchar(1);
	while (uart_bytes_received() != 1);

	uint8_t bumper_status = uart_get_byte(0);

	if (bumper_status == 0) {
		PORTD ^= _BV(PD4);
		return 0;
	}
	else {
		PORTD ^= _BV(PD5);
		return 1;
	}
}

int max(int a, int b) {
	if (a > b) {
		return a;
	}
	else if (b > a) {
		return b;
	}
	else {
		return a;
	}
}

int main(void) {

	clock8MHz();
	cli();
	//initialize leds
	DDRD = (1<<PORTD4) | (1<<PORTD5) | (1<<PORTD6) | (1<<PORTD7);

	Roomba_Init();
	Radio_Init();
	Radio_Configure_Rx(RADIO_PIPE_0, station1_addr, ENABLE);
	Radio_Configure(RADIO_2MBPS, RADIO_HIGHEST_POWER);

	sei();

	for (;;)
		{
			if (rxflag)
			{
				_delay_ms(20);
				//roomba_Drive(100, 0x0001);
				//roomba_Drive(0, 0);
				//PORTD ^= _BV(PD4);
				//PORTD ^= _BV(PD5);
				// Copy the received packet into the radio packet structure.  If there are no more packets,
				// then clear the rxflag so that the interrupt will set it next time a packet is received.
				if (Radio_Receive(&packet) != RADIO_RX_MORE_PACKETS)
				{
					rxflag = 0;
				}

				// If the packet is not a command, blink an error and don't do anything.
				if (packet.type != COMMAND)
				{
					//PORTD ^= _BV(PD7);
					continue;
				}

				if (packet.payload.command.command == START ||
						packet.payload.command.command == BAUD ||
						packet.payload.command.command == CONTROL ||
						packet.payload.command.command == SAFE ||
						packet.payload.command.command == FULL ||
						packet.payload.command.command == SENSORS)
				{
					// Don't pass the listed commands to the Roomba.
					continue;
				}

				// Set the radio's destination address to be the remote station's address
				Radio_Set_Tx_Addr(packet.payload.command.sender_address);

				int i=0;
				int north = 0;
				int south = 0;
				int east = 0;
				int west = 0;

				for (i=0; i<100; i++) {
					direction = get_current_direction();

					if (direction == IR_NORTH) {
						north = north + 1;
					}
					else if (direction == IR_SOUTH) {
						south = south + 1;
					}
					else if (direction == IR_EAST) {
						east = east + 1;
					}
					else if (direction == IR_WEST) {
						west = west + 1;
					}
				}

				int x = max(north,south);
				x = max(x,east);
				x = max(x,west);

				if (x == north) {
					direction = IR_NORTH;
				}
				if (x == south) {
					direction = IR_SOUTH;
				}
				if (x == east) {
					direction = IR_EAST;
				}
				if (x == west) {
					direction = IR_WEST;
				}

				// Update the Roomba sensors into the packet structure that will be transmitted.
				Roomba_UpdateSensorPacket(1, &packet.payload.sensors.sensors, direction);
				Roomba_UpdateSensorPacket(2, &packet.payload.sensors.sensors, 1);
				Roomba_UpdateSensorPacket(3, &packet.payload.sensors.sensors, 1);

				// send the sensor packet back to the remote station.
				packet.type = SENSOR_DATA;

				if (Radio_Transmit(&packet, RADIO_WAIT_FOR_TX) == RADIO_TX_MAX_RT)
				{
					//PORTD ^= _BV(PD4);	// flash red if the packet was dropped
				}
				else
				{
					//PORTD ^= _BV(PD5);	// flash green if the packet was received correctly
				}
			}
		}

		return 0;
	}

void radio_rxhandler(uint8_t pipenumber)
{
	rxflag = 1;
	//PORTD ^= _BV(PD7);
}


#endif
